#include "pid_control.h"
#include <chrono>
#include <memory>
#include <errno.h>
#include <fcntl.h> 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <iostream>
#include <time.h>
#include <math.h>

PIDControl::PIDControl(float p, float i, float d, float maxsum, float minoutput, float maxoutput)
{
	kp = p;
	ki = i;
	kd = d;
	MaxSum = maxsum;
	MinOutput = minoutput;
	MaxOutput = maxoutput;
	err = 0;
	olderr = 0;
	sumerr = 0;
}

float PIDControl::PID_Control(float preerr)
{
	float control;
	err = preerr;
	sumerr += err;
	if(sumerr > MaxSum)  sumerr = MaxSum;
	if(sumerr < -MaxSum) sumerr = -MaxSum;

	control = kp * err + ki*sumerr + kd*(err - olderr);
	olderr = err;
	if(control > MaxOutput)
		control = MaxOutput;
	if(control < MinOutput)
		control = MinOutput;
	return control;
}

